package com.example.utils;


/**
 * AIS工具类
 *
 * @author: Fred
 * @email 453086@qq.com
 * @create: 2024-09-29 08:32
 */
public class AISUtil1 {

    private static final double EARTH_RADIUS = 6371000;
    private static final double LATITUDE_TO_METER = 111120;
    private static final double[][] points = getPoints();


    /**
     * 获取点与点集最近的点索引
     *
     * @param targetPoint
     * @return
     */
    public static int getNearestPointIndex(double[] targetPoint) {

        int nearestPointIndex = 0;

        // 首先检查目标点是否与points中的某个点一致
        for (int i = 0; i < points.length; i++) {
            double[] point = points[i];
            if (Math.abs(point[0] - targetPoint[0]) < 1e-6 && Math.abs(point[1] - targetPoint[1]) < 1e-6) {
                return i;
            }
        }

        // 如果没有找到完全匹配的点，继续计算垂足并找最近点
        double minDistance = Double.MAX_VALUE;
        for (int i = 0; i < points.length - 1; i++) {
            double[] P1 = points[i];
            double[] P2 = points[i + 1];

            double x1 = P1[0], y1 = P1[1];
            double x2 = P2[0], y2 = P2[1];
            double x0 = targetPoint[0], y0 = targetPoint[1];

            double dx = x2 - x1;
            double dy = y2 - y1;
            double lenSquared = dx * dx + dy * dy;

            // 计算 t 的值
            double t = ((x0 - x1) * dx + (y0 - y1) * dy) / lenSquared;

            // 确保 t 在 [0, 1] 之间
            if (t < 0) {
                t = 0;
            }
            if (t > 1) {
                t = 1;
            }

            // 计算垂足的坐标
            double footX = x1 + t * dx;
            double footY = y1 + t * dy;

            // 计算目标点到垂足的距离
            double distance = Math.sqrt(Math.pow(footX - x0, 2) + Math.pow(footY - y0, 2));

            // 更新最近的垂足
            if (distance < minDistance) {
                minDistance = distance;
                nearestPointIndex = i;
            }
        }
        return nearestPointIndex;
    }

    public static double[] getNextPoint(double longitude, double latitude, double course, double speed) {

        int nearestPointIndex = getNearestPointIndex(longitude, latitude);

        // 确保索引有效
        if (nearestPointIndex < 0 || nearestPointIndex + 1 >= points.length) {
            return null;
        }

        // 获取当前点和下一个标准点
        double[] standardPoint = points[nearestPointIndex];
        double[] nextStandardPoint = points[nearestPointIndex + 1];

        // 计算方向向量（单位化）
        double[] directionVector = calculateDirectionVector(standardPoint, nextStandardPoint);

        // 计算新的经纬度
        double distanceMoved = speed * 0.514444 * 1;
        double newLatitude = latitude + (distanceMoved * directionVector[1]) / LATITUDE_TO_METER;
        double newLongitude = longitude + (distanceMoved * directionVector[0]) / (LATITUDE_TO_METER * Math.cos(Math.toRadians(latitude)));

        return new double[]{newLongitude, newLatitude};
    }

    /**
     * 计算从点A到点B的方向向量（单位化）。
     */
    private static double[] calculateDirectionVector(double[] pointA, double[] pointB) {
        double latDiff = (pointB[1] - pointA[1]) * LATITUDE_TO_METER;
        double lonDiff = (pointB[0] - pointA[0]) * LATITUDE_TO_METER * Math.cos(Math.toRadians(pointA[1]));

        double magnitude = Math.sqrt(latDiff * latDiff + lonDiff * lonDiff);

        return new double[]{lonDiff / magnitude, latDiff / magnitude};
    }


    public static int getNearestPointIndex(double longitude, double latitude) {

        int nearestPointIndex = 0;

        // 计算转换因子，避免每次循环重复计算
        double latConversionFactor = LATITUDE_TO_METER;
        double lonConversionFactor = LATITUDE_TO_METER * Math.cos(Math.toRadians(latitude));
        double minDistance = Double.MAX_VALUE;
        for (int i = 0; i < points.length; i++) {

            double[] standardPoint = points[i];

            // 提取当前点与标准点的经纬度
            double latDiff = (latitude - standardPoint[1]) * latConversionFactor;
            double lonDiff = (longitude - standardPoint[0]) * lonConversionFactor;

            // 计算两点之间的直线距离
            double distance = Math.sqrt(latDiff * latDiff + lonDiff * lonDiff);

            // 更新最近的点
            if (distance < minDistance) {
                minDistance = distance;
                nearestPointIndex = i;
            }
        }
        return nearestPointIndex;
    }


    /**
     * 获取点到线最近的两个点
     *
     * @param targetPoint
     * @return
     */
    public static double[][] getNearestTwoPoints(double[] targetPoint) {

        int nearestPointIndex = getNearestPointIndex(targetPoint);

        // 处理边界情况
        double[][] nearestPoints = new double[2][2];

        if (nearestPointIndex == 0) {
            // 最近点为第一个点，返回第一个和第二个点
            nearestPoints[0][0] = points[0][0];
            nearestPoints[0][1] = points[0][1];
            nearestPoints[1][0] = points[1][0];
            nearestPoints[1][1] = points[1][1];
        } else if (nearestPointIndex == points.length - 1) {
            // 最近点为最后一个点，返回倒数第二个和最后一个点
            nearestPoints[0][0] = points[nearestPointIndex][0];
            nearestPoints[0][1] = points[nearestPointIndex][1];
            nearestPoints[1][0] = points[nearestPointIndex + 1][0];
            nearestPoints[1][1] = points[nearestPointIndex + 1][1];
        } else {
            // 返回最近点前后的两个点
            nearestPoints[0][0] = points[nearestPointIndex][0];
            nearestPoints[0][1] = points[nearestPointIndex][1];
            nearestPoints[1][0] = points[nearestPointIndex + 1][0];
            nearestPoints[1][1] = points[nearestPointIndex + 1][1];
        }

        double[] pointA = targetPoint;
        double[] X1 = new double[]{nearestPoints[0][0], nearestPoints[0][1]};
        double[] X2 = new double[]{nearestPoints[1][0], nearestPoints[1][1]};

        return nearestPoints;
    }

    /**
     * 计算点到线的距离
     *
     * @param targetPoint
     * @return
     */
    public static double calculateDistancePointToLine(double[] targetPoint) {
        // 获取最近的两点
        double[][] nearestPoints = AISUtil1.getNearestTwoPoints(targetPoint);
        // 获取点与线的垂直点
        double[] pointB = AISUtil1.getPerpendicularPoint(targetPoint, nearestPoints);

        return haversineDistance(targetPoint, pointB);
    }


    /**
     * 获取 A点到线的垂足
     */
    public static double[] getPerpendicularPoint(double[] pointA, double[][] nearestPoints) {
        // 将经纬度转换为三维空间坐标
        double[] A = latLonToCartesian(pointA);
        double[] B = latLonToCartesian(nearestPoints[0]);
        double[] C = latLonToCartesian(nearestPoints[1]);

        // 计算向量 BC 和 BA
        double[] BC = new double[]{C[0] - B[0], C[1] - B[1], C[2] - B[2]};
        double[] BA = new double[]{A[0] - B[0], A[1] - B[1], A[2] - B[2]};

        // 计算 BC 的单位向量
        double bcLength = Math.sqrt(BC[0] * BC[0] + BC[1] * BC[1] + BC[2] * BC[2]);
        double[] unitBC = new double[]{BC[0] / bcLength, BC[1] / bcLength, BC[2] / bcLength};

        // 计算 A 到 B 的向量与 BC 的点积，得到投影长度
        double projectionLength = (BA[0] * unitBC[0] + BA[1] * unitBC[1] + BA[2] * unitBC[2]);

        // 计算垂足点 D 的坐标
        double[] D = new double[]{
                B[0] + projectionLength * unitBC[0],
                B[1] + projectionLength * unitBC[1],
                B[2] + projectionLength * unitBC[2]
        };
        // 将 D 点从三维空间坐标转换回经纬度
        return cartesianToLatLon(D);
    }

    /**
     * 将经纬度转换为三维空间坐标
     */
    public static double[] latLonToCartesian(double[] latLon) {
        double lat = Math.toRadians(latLon[1]);
        double lon = Math.toRadians(latLon[0]);

        double x = EARTH_RADIUS * Math.cos(lat) * Math.cos(lon);
        double y = EARTH_RADIUS * Math.cos(lat) * Math.sin(lon);
        double z = EARTH_RADIUS * Math.sin(lat);

        return new double[]{x, y, z};
    }

    /**
     * 将三维空间坐标转换为经纬度
     */
    public static double[] cartesianToLatLon(double[] cartesian) {
        double x = cartesian[0];
        double y = cartesian[1];
        double z = cartesian[2];

        double lon = Math.atan2(y, x);
        double hyp = Math.sqrt(x * x + y * y);
        double lat = Math.atan2(z, hyp);

        return new double[]{Math.toDegrees(lon), Math.toDegrees(lat)};
    }


    /**
     * 计算从 B 点生成的垂直点 C
     */
    public static double[] getLinePerpendicularPoint(double[] pointA, double[] pointB, double[][] nearestPoints, double speed, double course) {

        double lonBRad = Math.toRadians(pointB[0]);
        double latBRad = Math.toRadians(pointB[1]);

        // 计算AB的方位角
        double bearing = calculateBearing(pointA, pointB);
        // 计算中心线最近2个点的方位角
        double nearestBearing = calculateBearing(nearestPoints[0], nearestPoints[1]);
        // 计算垂直于AB的方位角
        double perpBearing = isWithinRange(course, nearestBearing) ? bearing + Math.PI / 2 : bearing - Math.PI / 2;

        // 计算1秒后角距离（弧度）
        double distance = speed * 0.51444 * 1;
        double angularDistance = distance / EARTH_RADIUS;

        // 计算C点的坐标
        double latC = Math.asin(Math.sin(latBRad) * Math.cos(angularDistance) +
                Math.cos(latBRad) * Math.sin(angularDistance) * Math.cos(perpBearing));
        double lonC = lonBRad + Math.atan2(Math.sin(perpBearing) * Math.sin(angularDistance) * Math.cos(latBRad),
                Math.cos(angularDistance) - Math.sin(latBRad) * Math.sin(latC));

        double[] pointC = new double[]{Math.toDegrees(lonC), Math.toDegrees(latC)};

        if (AISUtil1.isPointOutsideLineSegment(pointC, nearestPoints)) {
            System.out.println("pointC不在线上X1X2");
            //  pointC = nearestPoints[1];
        }
        return pointC;
    }

    public static boolean isPointOutsideLineSegment(double[] pointC, double[][] nearestPoints) {

        double[] pointA = nearestPoints[0];
        double[] pointB = nearestPoints[1];

        // 首先检查C是否在直线AB上
        double crossProduct = (pointB[0] - pointA[0]) * (pointC[1] - pointA[1]) -
                (pointB[1] - pointA[1]) * (pointC[0] - pointA[0]);

        double distanceAB = haversineDistance(pointA, pointB);
        double distanceAC = haversineDistance(pointA, pointC);

        if (Math.abs(crossProduct) < 1e-10) {
            // 如果AC > AB，说明C在外面
            System.out.println(distanceAC);
            System.out.println(distanceAB);

            return distanceAC > distanceAB;
        }
        return true;
    }

    /**
     * 根据给定的点A, B, C计算长方形的D点
     *
     * @param pointA 点A
     * @param pointB 点B
     * @param pointC 点C
     * @return 点D的经纬度
     */
    public static double[] getRectangleLastPoint(double[] pointA, double[] pointB, double[] pointC, double distance) {

        // 计算D点的经纬度
        double lonD = pointC[0] - (pointB[0] - pointA[0]);
        double latD = pointC[1] - (pointB[1] - pointA[1]);
        double[] pointD = new double[]{lonD, latD};

        if (Math.abs(haversineDistance(pointC, pointD) - distance) > 0.1) {
            return getNewPointD1(pointC, pointD, distance);
        }

        return new double[]{lonD, latD};
    }

    /**
     * 通过线性插值法沿着A到D的方向延长到D点
     *
     * @param pointC
     * @param pointD
     * @param distance
     * @return
     */
    public static double[] getNewPointD1(double[] pointC, double[] pointD, double distance) {
        // 计算C到D的距离
        double distanceCD = haversineDistance(pointC, pointD);

        // 计算C到D的经纬度差
        double lonDiff = pointD[0] - pointC[0];
        double latDiff = pointD[1] - pointC[1];

        // 计算CD之间的单位向量
        double unitLonDiff = lonDiff / distanceCD;
        double unitLatDiff = latDiff / distanceCD;

        // 计算新点D1的经纬度
        double newLatD1 = pointC[1] + unitLatDiff * distance;
        double newLonD1 = pointC[0] + unitLonDiff * distance;

        return new double[]{newLonD1, newLatD1};
    }


    /**
     * 计算方位角
     *
     * @param point1
     * @param point2
     * @return
     */
    private static double calculateBearing(double[] point1, double[] point2) {
        double dLon = Math.toRadians(point2[0] - point1[0]);
        double lat1 = Math.toRadians(point1[1]);
        double lat2 = Math.toRadians(point2[1]);
        double y = Math.sin(dLon) * Math.cos(lat2);
        double x = Math.cos(lat1) * Math.sin(lat2) - Math.sin(lat1) * Math.cos(lat2) * Math.cos(dLon);
        return Math.atan2(y, x);
    }

    public static boolean isWithinRange(double course, double bearing) {
        // 将 course 和 angle 都限制在 0 到 360 之间
        course = (course + 360) % 360;
        bearing = (bearing + 360) % 360;

        if (bearing > 270) {
            // 当 angle 大于 270 时，定义角度范围
            double lowerBound = (bearing - 90 + 360) % 360;
            double upperBound = (bearing + 90) % 360;

            // 判断是否在范围内（跨越0度的情况）
            return (course >= lowerBound || course <= upperBound);
        } else if (bearing > 0) {
            return course >= 0 && course <= bearing;
        }
        return false;
    }


    public static double haversineDistance(double[] point1, double[] point2) {
        final double R = EARTH_RADIUS;
        double lat1 = Math.toRadians(point1[1]);
        double lon1 = Math.toRadians(point1[0]);
        double lat2 = Math.toRadians(point2[1]);
        double lon2 = Math.toRadians(point2[0]);

        double dLat = lat2 - lat1;
        double dLon = lon2 - lon1;

        double a = Math.sin(dLat / 2) * Math.sin(dLat / 2) +
                Math.cos(lat1) * Math.cos(lat2) *
                        Math.sin(dLon / 2) * Math.sin(dLon / 2);
        double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
        return R * c;
    }

    private static double[][] getPoints() {
        double[][] line = {{119.4596528222393,29.19720150937384},{119.45930413506738,29.19686902671549},{119.45896617673151,29.19653186010177},{119.45861748955956,29.196180643700146},{119.45826880238764,29.195857523548142},{119.45788256428949,29.195562499911375},{119.4574909617733,29.19526279249058},{119.45709935925719,29.194958401244662},{119.45672384999507,29.194668057983463},{119.45635906956906,29.194321518176224},{119.45603720448726,29.193993709172588},{119.45579044125788,29.193586287950296},{119.45559732220885,29.193155449954318},{119.45539883874173,29.192766757600438},{119.455264728291,29.192307817743725},{119.45518835699819,29.191880799504418},{119.4550971618917,29.19145463745248},{119.45500060236715,29.191009741113334},{119.45493086493279,29.19055079339367},{119.45486649191643,29.190091843620223},{119.45478602564604,29.189623525407825},{119.45473774588379,29.189211403611964},{119.45468946612154,29.188775864005194},{119.45460363543305,29.188349689051293},{119.45455535567085,29.187890729428304},{119.45450171149056,29.18744113433702},{119.45446416056436,29.18697748733507},{119.4544319740562,29.186561452130164},{119.45440515196606,29.186107167859106},{119.45435687220379,29.185648198201932},{119.4543032280235,29.18516112611563},{119.45424421942518,29.1847115190602},{119.45420020166456,29.184303932045154},{119.45417337957443,29.183854321232758},{119.45413582864822,29.183409391926475},{119.45410900655804,29.18297382768381},{119.45405138567884,29.18251032328142},{119.4540138347526,29.182060704608247},{119.4539601905723,29.181602724701392},{119.4539226396461,29.181162469209312},{119.45388508871989,29.18071752822877},{119.45383680895763,29.18025385083797},{119.45380462244945,29.179822956838777},{119.45374561385111,29.179347353357315},{119.4536919696708,29.178916455551203},{119.45361686781838,29.17845745372385},{119.45351494387587,29.17799844984323},{119.45338619784314,29.17762375127566},{119.45322526530228,29.177174111188464},{119.45303751067122,29.176771306937283},{119.45288730696639,29.176345082112547},{119.45272101000748,29.175928223154003},{119.45252252654038,29.1754411049221},{119.45235086516344,29.175052345443376},{119.45218456820452,29.174638363619394},{119.45196462706531,29.17420744603922},{119.4517638130641,29.1738225591601},{119.45154923634291,29.173429109618098},{119.4513132019496,29.17303097461756},{119.45107180313829,29.172632838072587},{119.45083040432696,29.172239383969654},{119.45059973435168,29.171836560349263},{119.45034224228628,29.171438419170954},{119.45006865696675,29.171063696651146},{119.4497360630489,29.170688972763287},{119.44945174889341,29.170356404166952},{119.44907623963132,29.169948889065708},{119.44875973896754,29.169658475017698},{119.44840568737756,29.169358691912777},{119.44800872044338,29.16905890793231},{119.4476171179272,29.16878254379975},{119.4471826000668,29.168510863079774},{119.44678026871455,29.168290707486165},{119.44626528458372,29.16807523559693},{119.44582003788724,29.167887868369064},{119.44530505375634,29.167719237571617},{119.44485980705987,29.16762086947851},{119.44435555176506,29.167485027671148},{119.44385666088827,29.16744286983233},{119.44333631233944,29.167391343561334},{119.44282132820861,29.167358554102705},{119.44232780174984,29.167311712000767},{119.44182354645505,29.167307027789406},{119.4412602825619,29.16732108042285},{119.44069701866877,29.167330448844126},{119.44022494988215,29.167363238311708},{119.43975288109552,29.16740539618327},{119.43923513049077,29.167453027118796},{119.43872014635991,29.167504553358796},{119.43818906897496,29.16758418476978},{119.43770090693425,29.167621658353568},{119.43715373629524,29.167649763532438},{119.43670312518074,29.16769660548009},{119.43616379484497,29.167766868361568},{119.43563808187805,29.16780434187865},{119.43510700449312,29.167865236314768},{119.43462957128848,29.16793549908077},{119.43415750250183,29.167987025078617},{119.43360496744478,29.1680619719384},{119.4330792544779,29.16811818204731},{119.43256963476506,29.168174392125493},{119.43206001505224,29.16820249715302},{119.43157185301152,29.168254023016843},{119.43104927078474,29.168318443012566},{119.43051282898178,29.16836528465514},{119.4300675822853,29.168454283717097}};
        return line;
    }
}
